import glob
import subprocess
import time
from ctypes import *
from datetime import datetime
from os import mkdir
import re

# from typing import re

import cv2
import numpy as np
import math
import os

from RobotArmAPI import arm
from take_photo import camera_capture
xmlPath = "F:\\GTAPI(3)\\GTAPI\\Randn_Marker_cascade.xml"
def rotate_by_z(c_x, c_y, theta_z):
    rz = theta_z * math.pi / 180.0
    out_x = math.cos(rz) * c_x - math.sin(rz) * c_y
    out_y = math.sin(rz) * c_x + math.cos(rz) * c_y
    return out_x, out_y


def rotate_by_y(c_x, c_z, theta_y):
    ry = theta_y * math.pi / 180.0
    out_z = math.cos(ry) * c_z - math.sin(ry) * c_x
    out_x = math.sin(ry) * c_z + math.cos(ry) * c_x
    return out_x, out_z


def rotate_by_x(c_y, c_z, theta_x):
    rx = theta_x * math.pi / 180.0
    out_y = math.cos(rx) * c_y - math.sin(rx) * c_z
    out_z = math.sin(rx) * c_y + math.cos(rx) * c_z
    return out_y, out_z
def auto_align_rto(file_dir, chartId, chart_x, chart_y, distance, thresh, blurKernel, minRto, maxRto, ratio):
    src = cv2.imread(file_dir)

    cols = src.shape[1]
    rows = src.shape[0]
    print('cols', cols, rows)
    min_size = int(rows * minRto)
    max_size = int(rows * maxRto)
    print(min_size, max_size)
    src_g = cv2.GaussianBlur(src, (blurKernel, blurKernel), 0)
    gray = cv2.cvtColor(src_g, cv2.COLOR_BGR2GRAY)
    retval, dst = cv2.threshold(gray, thresh, 255, cv2.THRESH_BINARY)
    dst = cv2.equalizeHist(dst)

    marker_cascade = cv2.CascadeClassifier(xmlPath)
    # marker_points = np.zeros((6, 2), dtype=np.double)
    marker_points_x = []
    marker_points_y = []
    dis = []
    dist = []
    dis_x = []
    dis_y = []
    result_out = dict()

    # for deadleaf chart, marker flip
    if chartId == 1 or chartId == 7 or chartId == 9:
        marker_rect = marker_cascade.detectMultiScale(dst, 1.1, 1, 0, (min_size, min_size), (max_size, max_size))
        print('marker_rect', marker_rect)
        for i in range(len(marker_rect)):
            (x, y, w, h) = marker_rect[i]
            marker_points_x.append(x + w / 2)
            marker_points_y.append(y + h / 2)
        count = len(marker_rect)

        dst = cv2.transpose(dst)
        dst = cv2.flip(dst, 0)
        # cv2.imwrite("D://test_photo//testFlip.jpg", dst)
        saveFile = r'F:\red\110.jpg'  # 保存文件的路径
        # cv2.imwrite(saveFile, img, [int(cv2.IMWRITE_PNG_COMPRESSION), 8])  # 保存图像文件, 设置压缩比为 8
        # cv2.imwrite(saveFile, img)
        cv2.imencode('.jpg', dst)[1].tofile(saveFile)

        marker_rect_r = marker_cascade.detectMultiScale(dst, 1.1, 1, 0, (min_size, min_size), (max_size, max_size))
        for i in range(len(marker_rect_r)):
            (x, y, w, h) = marker_rect_r[i]
            if count + i > 8:
                result_out['flag'] = -2
                return result_out
            marker_points_x.append(cols - y - h / 2)
            marker_points_y.append(x + w / 2)

        if len(marker_points_x) < 4:
            result_out['flag'] = -4
            return result_out

    # for normal chart
    else:
        marker_rect = marker_cascade.detectMultiScale(dst, 1.1, 1, 0, (min_size, min_size), (max_size, max_size))
        print(marker_rect)
        if len(marker_rect) < 4:
            result_out['flag'] = -4
            return result_out

        for i in range(len(marker_rect)):
            (x, y, w, h) = marker_rect[i]
            marker_points_x.append(x + w / 2)
            marker_points_y.append(y + h / 2)
            # cv2.circle(src_g, (int(x + w / 2), int(y + h / 2)), 7, (0, 0, 255))

    # point sort
    for i in range(len(marker_points_x)):
        dis.append(math.sqrt(marker_points_x[i] ** 2 + marker_points_y[i] ** 2))
        dist.append(math.sqrt((marker_points_x[i] - cols + 1) ** 2 + marker_points_y[i] ** 2))
        dis_x.append(marker_points_x[i])
        dis_y.append(marker_points_y[i])

    m_min = dis[0]
    m_max = dis[0]
    min_id = 0
    max_id = 0

    min_t = dist[0]
    max_t = dist[0]
    min_t_id = 0
    max_t_id = 0

    for i in range(1, len(dis)):
        if dis[i] <= m_min:
            m_min = dis[i]
            min_id = i
        if dis[i] > m_max:
            m_max = dis[i]
            max_id = i

    for m in range(1, len(dist)):
        if dist[m] <= min_t:
            min_t = dist[m]
            min_t_id = m
        if dist[m] > max_t:
            max_t = dist[m]
            max_t_id = m

    # 2d pixel axis
    object_2d_points = np.array(([marker_points_x[min_id], marker_points_y[min_id]],
                                 [marker_points_x[min_t_id], marker_points_y[min_t_id]],
                                 [marker_points_x[max_id], marker_points_y[max_id]],
                                 [marker_points_x[max_t_id], marker_points_y[max_t_id]]), dtype=np.double)

    cv2.circle(src_g, (int(marker_points_x[min_id]), int(marker_points_y[min_id])), 20, (0, 0, 255))
    cv2.circle(src_g, (int(marker_points_x[min_t_id]), int(marker_points_y[min_t_id])), 20, (0, 0, 255))
    cv2.circle(src_g, (int(marker_points_x[max_id]), int(marker_points_y[max_id])), 20, (0, 0, 255))
    cv2.circle(src_g, (int(marker_points_x[max_t_id]), int(marker_points_y[max_t_id])), 20, (0, 0, 255))
    # cv2.imwrite("D:\\RandnAnalyzer\\testMarker.jpg", src_g)
    saveFile = r'F:/red/111.jpg'  # 保存文件的路径
    # cv2.imwrite(saveFile, img, [int(cv2.IMWRITE_PNG_COMPRESSION), 8])  # 保存图像文件, 设置压缩比为 8
    # cv2.imwrite(saveFile, img)
    cv2.imencode('.jpg', src_g)[1].tofile(saveFile)
    # 3d object axis
    object_3d_points = np.array(([0, 0, 0],
                                 [chart_x, 0, 0],
                                 [chart_x, chart_y, 0],
                                 [0, chart_y, 0]), dtype=np.double)

    pixel_dis_w = math.sqrt((object_2d_points[1][0] - object_2d_points[0][0]) ** 2 + (object_2d_points[1][1] -
                                                                                      object_2d_points[0][1]) ** 2)
    pixel_dis_h = math.sqrt((object_2d_points[3][0] - object_2d_points[0][0]) ** 2 + (object_2d_points[3][1] -
                                                                                      object_2d_points[0][1]) ** 2)
    # 内参估计
    fx = pixel_dis_w / chart_x * distance
    fy = pixel_dis_h / chart_y * distance
    cx = cols / 2
    cy = rows / 2
    camera_matrix = np.array(([fx, 0, cx],
                              [0, fy, cy],
                              [0, 0, 1.0]), dtype=np.double)
    dist_parameter = np.array([0.0, 0.0, 0.0, 0.0, 0.0], dtype=np.double)

    # PNP & axis transform
    find, r_vec, t_vec = cv2.solvePnP(object_3d_points, object_2d_points, camera_matrix, dist_parameter,
                                      flags=cv2.SOLVEPNP_EPNP)
    rot_m = cv2.Rodrigues(r_vec)[0]
    camera_position = -np.matrix(rot_m).T * np.matrix(t_vec)
    angles = []
    theta_z = math.atan2(rot_m[1, 0], rot_m[0, 0]) * 180.0 / math.pi
    theta_y = math.atan2(-1.0 * rot_m[2, 0], math.sqrt(rot_m[2, 1] ** 2 + rot_m[2, 2] ** 2)) * 180.0 / math.pi
    theta_x = math.atan2(rot_m[2, 1], rot_m[2, 2]) * 180.0 / math.pi
    # print("theta_xyz: ", theta_x, theta_y, theta_z)
    x = t_vec[0]
    y = t_vec[1]
    z = t_vec[2]
    (x, y) = rotate_by_z(x, y, -1.0 * theta_z)
    (x, z) = rotate_by_y(x, y, -1.0 * theta_y)
    (y, z) = rotate_by_x(y, z, -1.0 * theta_x)
    angles.append(-1 * theta_x)
    angles.append(-1 * theta_y)
    angles.append(-1 * theta_z)
    trans = []
    actual_dist = -1 * z
    center_x = chart_x / 2
    center_y = chart_y / 2
    out_x = (-1 * x) - center_x
    out_y = (-1 * y) - center_y
    rate_x = chart_x / cols
    rate_y = chart_y / rows
    if rate_x >= rate_y:
        out_dis = chart_y * actual_dist / cols
    else:
        out_dis = chart_x * actual_dist / (rows * ratio)
    out_z = actual_dist - out_dis
    trans.append(out_x)
    trans.append(out_y)
    trans.append(out_z)

    result_out['flag'] = 1
    result_out['angles'] = angles
    result_out['trans'] = trans
    result_out['actual_dist'] = actual_dist
    print(result_out)
    return result_out


def red_detect(img_path):
    # 载入原图
    img = cv2.imread(img_path)

    result = cv2.GaussianBlur(img, (15, 15), 15)

    grid_RGB = cv2.cvtColor(result, cv2.COLOR_BGR2RGB)

    # 从RGB色彩空间转换到HSV色彩空间
    grid_HSV = cv2.cvtColor(grid_RGB, cv2.COLOR_RGB2HSV)

    # H、S、V范围一：
    lower1 = np.array([0, 130, 140])
    upper1 = np.array([10, 255, 255])
    mask1 = cv2.inRange(grid_HSV, lower1, upper1)  # mask1 为二值图像

    # H、S、V范围二：
    lower2 = np.array([175, 130, 140])
    upper2 = np.array([180, 255, 255])
    mask2 = cv2.inRange(grid_HSV, lower2, upper2)

    # 将两个二值图像结果 相加
    mask3 = mask1 + mask2

    # 腐蚀操作将一些斑点消除
    conv_kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (5, 5))  # 生成5x5的全1矩阵
    mask3 = cv2.erode(mask3, conv_kernel)  # 腐蚀

    # 查找轮廓
    cnts, hierarchy = cv2.findContours(mask3, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)


    centerdisorder = []
    for i, c in enumerate(cnts):
        perimeter = np.around(cv2.arcLength(c, True), decimals=3)
        area = np.around(cv2.contourArea(c), decimals=3)

        # compute the center of the contour
        M = cv2.moments(c)


        if perimeter<20:
            continue

        if area<40:
            continue

        a = sorted(c[:, 0], key=lambda x: x[0])  # 所有坐标按x轴从小到大排序
        x_min = a[0][0]
        x_max = a[-1][0]
        b = sorted(c[:, 0], key=lambda x: x[1])  # 所有坐标按y轴从小到大排序
        y_min = b[0][1]
        y_max = b[-1][1]

        ratio = (x_max-x_min)/(y_max-y_min)

        if not (0.7<=ratio<=1.3):
            continue


        s = (x_max-x_min)*(y_max-y_min)
        ratio1 = area/s

        if (ratio1<=0.7):
            continue

        if  M["m00"]==0:
            continue
        cX = int(M["m10"] / M["m00"])
        cY = int(M["m01"] / M["m00"])

        disorder = []
        disorder.append(cX)
        disorder.append(cY)
        disorder.append(area)

        disorder.append(x_min)
        disorder.append(x_max)
        disorder.append(y_min)
        disorder.append(y_max)


        centerdisorder.append(disorder)

    centerdisorder.sort(key=lambda ele: ele[1], reverse=False)#对中心点y坐标排序

    center = []
    if len(centerdisorder)<4:
        return center

    if centerdisorder[0][0]<centerdisorder[1][0]:
        areaD = centerdisorder[0][2]
    else:
        areaD = centerdisorder[1][2]


    centera = []
    #判断轮廓的面积是否达标
    for i in range(len(centerdisorder)):
        if centerdisorder[i][2]>areaD/2:
            centera.append(centerdisorder[i])


    # 对按y排序后的前两个和后两个点放入删选列表中
    center0 = []
    centera.sort(key=lambda ele: ele[1], reverse=False)  # 对中心点y坐标排序
    center0.append(centera[0])
    center0.append(centera[1])
    center0.append(centera[-1])
    center0.append(centera[-2])

    center0.sort(key=lambda ele: ele[1], reverse=False)  # 对中心点y坐标排序

    # 对按y排序后的前两个和后两个点放入删选列表中
    center0 = []
    center2 = []
    centera.sort(key=lambda ele: ele[1], reverse=False)  # 对中心点y坐标排序
    center0.append(centera[0])
    center0.append(centera[1])
    center0.append(centera[-1])
    center0.append(centera[-2])

    center0.sort(key=lambda ele: ele[1], reverse=False)  # 对中心点y坐标排序
    print(center0)
    # 对四个点按照左上，右上，右下，左下的顺序进行排序
    if center0[0][0] > center0[1][0]:
        app1 = [center0[1][0], center0[1][1]]
        app11 = [center0[1][4], center0[1][5]]
        # 将中心点坐标和顶点坐标存入列表
        center.append(app1)
        center2.append(app11)

        app2 = [center0[0][0], center0[0][1]]
        app22 = [center0[0][3], center0[0][5]]
        center.append(app2)
        center2.append(app22)
    else:
        app1 = [center0[0][0], center0[0][1]]
        app11 = [center0[0][4], center0[0][5]]
        center.append(app1)
        center2.append(app11)

        app2 = [center0[1][0], center0[1][1]]
        app22 = [center0[1][3], center0[1][5]]
        center.append(app2)
        center2.append(app22)

    if center0[2][0] > center0[3][0]:
        app1 = [center0[2][0], center0[2][1]]
        app11 = [center0[2][3], center0[2][6]]
        center.append(app1)
        center2.append(app11)

        app2 = [center0[3][0], center0[3][1]]
        app22 = [center0[3][4], center0[3][6]]
        center.append(app2)
        center2.append(app22)

    else:
        app1 = [center0[3][0], center0[3][1]]
        app11 = [center0[3][3], center0[3][6]]
        center.append(app1)
        center2.append(app11)

        app2 = [center0[2][0], center0[2][1]]
        app22 = [center0[2][4], center0[2][6]]
        center.append(app2)
        center2.append(app22)

    return center, center2
def auto_align_red(chart_x, chart_y, distance, file_dir):
    result_out = dict()
    # img_path = './red/IMG_20220909_142324.jpg'

    img_path = file_dir
    # res[i][0]: 四个点的x坐标
    # res[i][1]: 四个点的y坐标

    res, res1 = red_detect(img_path)

        # res, res1 = red_detect(img_path)
    # res1 = res
    # print(res)

    print(res)
    # res = [[473, 491], [3523, 334], [3638, 2357], [550, 2494]]
    img = cv2.imread(img_path)
    cols = img.shape[1]
    rows = img.shape[0]
    object_2d_points = np.array(res, dtype=np.double)
    for i in range(len(res)):
        cv2.circle(img, (res[i][0], res[i][1]), 7, (0, 255, 0), -1)

    saveFile = r'F:\red\110.jpg'  # 保存文件的路径
    # cv2.imwrite(saveFile, img, [int(cv2.IMWRITE_PNG_COMPRESSION), 8])  # 保存图像文件, 设置压缩比为 8
    # cv2.imwrite(saveFile, img)
    cv2.imencode('.jpg', img)[1].tofile(saveFile)
    # cv2.namedWindow("drawimg", cv2.WINDOW_KEEPRATIO)
    # cv2.imshow('drawimg', img)
    # cv2.waitKey(0)
    # cv2.destroyAllWindows()
    # 3d object axis
    object_3d_points = np.array(([0, 0, 0],
                                 [chart_x, 0, 0],
                                 [chart_x, chart_y, 0],
                                 [0, chart_y, 0]), dtype=np.double)

    pixel_dis_w = math.sqrt((object_2d_points[1][0] - object_2d_points[0][0]) ** 2 + (object_2d_points[1][1] -
                                                                                      object_2d_points[0][1]) ** 2)

    pixel_dis_h = math.sqrt((object_2d_points[3][0] - object_2d_points[0][0]) ** 2 + (object_2d_points[3][1] -
                                                                                      object_2d_points[0][1]) ** 2)
    # 内参估计
    fx = float(pixel_dis_w) / float(chart_x) * float(distance)
    # print(fx)
    fy = float(pixel_dis_h) / float(chart_y) * float(distance)
    # print(fy)
    cx = cols / 2
    cy = rows / 2
    camera_matrix = np.array(([fx, 0, cx],
                              [0, fy, cy],
                              [0, 0, 1.0]), dtype=np.double)
    dist_parameter = np.array([0.0, 0.0, 0.0, 0.0, 0.0], dtype=np.double)

    # PNP & axis transform
    find, r_vec, t_vec = cv2.solvePnP(object_3d_points, object_2d_points, camera_matrix, dist_parameter,
                                      flags=cv2.SOLVEPNP_EPNP)
    rot_m = cv2.Rodrigues(r_vec)[0]
    camera_position = -np.matrix(rot_m).T * np.matrix(t_vec)
    angles = []
    theta_z = math.atan2(rot_m[1, 0], rot_m[0, 0]) * 180.0 / math.pi
    theta_y = math.atan2(-1.0 * rot_m[2, 0], math.sqrt(rot_m[2, 1] ** 2 + rot_m[2, 2] ** 2)) * 180.0 / math.pi
    theta_x = math.atan2(rot_m[2, 1], rot_m[2, 2]) * 180.0 / math.pi
    # print("theta_xyz: ", theta_x, theta_y, theta_z)
    x = t_vec[0]
    y = t_vec[1]
    z = t_vec[2]
    (x, y) = rotate_by_z(x, y, -1.0 * theta_z)
    (x, z) = rotate_by_y(x, y, -1.0 * theta_y)
    (y, z) = rotate_by_x(y, z, -1.0 * theta_x)
    angles.append(-1 * theta_x)
    angles.append(-1 * theta_y)
    angles.append(-1 * theta_z)
    trans = []
    actual_dist = -1 * z
    center_x = int(chart_x) / 2
    center_y = int(chart_y) / 2
    out_x = (-1 * x) - center_x
    out_y = (-1 * y) - center_y
    rate_x = chart_x / cols
    rate_y = chart_y / rows
    if rate_x >= rate_y:
        out_dis = chart_y * actual_dist / cols
    else:
        out_dis = chart_x * actual_dist / rows
    out_z = actual_dist - out_dis
    trans.append(out_x)
    trans.append(out_y)
    trans.append(out_z)
    print('out_z', out_z)
    result_out['flag'] = 1
    result_out['angles'] = angles
    result_out['trans'] = trans
    result_out['actual_dist'] = actual_dist
    result_out['res'] = res1
    # print(result_out)
    return result_out

if __name__ == '__main__':
    Visual_Noise = [{'id': 1, 'is_marker': 0, 'marker_x': 700, 'marker_y': 400, 'distance': 1400, 'threshold': 130,
                     'blurkernel': 3, 'minarea': 0.1, 'maxarea': 0.2, 'ratio': 2.7}]
    auto_align_rto(r'frame29.jpg',1,700,400,1400,70,3,0.1,0.2,1)